/*
 * Cpu.cpp
 *
 *  Created on: Dec 24, 2009
 *      Author: artur
 */

#include <kiddie/kiddie.h>
#include <platform.h>
#include <sys/errno.h>

#include <Console.h>
#include <hw/Platform.h>
#include <mem/page/PageFactory.h>

extern kernel::Console cout;
extern kernel::Platform hwplatform;

namespace kernel
{
  namespace mem
  {
    namespace page
    {

      PageFactory::PageFactory()
      {
      }

      PageFactory::~PageFactory()
      {
      }

      int
      PageFactory::init(void)
      {
        pgd_t pmd, vpmd;
        pgd_t pgd, vpgd;
        pgd_t _pgd2;
        u32 paddr = 0;
        u32 i, j, k;

        struct hw_profile *hwpp = hwplatform.bootInfo;
        struct p_block *blk;
        void *k_base = &KERNEL_BASE;
        void *k_end = &KERNEL_END;

        ::cout << kernel::Console::HEX;
        cout << "[ PageFactory ] init(void)\n";

        this->page_head.size = 0;
        this->page_head.next = &this->page_head;
        this->page_head.prev = &this->page_head;
        if (hwpp->bootmem.size != 0)
          {
            ::cout << "[ PageFactory ] bootmem.size!! > 0\n";
            blk = (struct ::p_block *) hwpp->bootmem.start;
            blk = (struct ::p_block *) phys_to_virt(blk);
            blk->size = hwpp->bootmem.size;
            this->page_head.next = blk;
            this->page_head.prev = blk;
            blk->next = &this->page_head;
            blk->prev = &this->page_head;
            total_bytes = blk->size;

            k_base = virt_to_phys(k_base);
            k_end = virt_to_phys(k_end);
            this->reserve((void *) k_base, (size_t)((char *) k_end
                - (char *) k_base));
          }

        /*
         * TODO This is platform dependent code!!!!!
         * Here we will remap early allocated map into new one.
         * Old map will be lost, so don't care about freeing that memory.
         * Allocate pages for 8MB mapping - * 1 page for pgd, 8 pages for mid page tables
         */
        pgd = (pgd_t) this->alloc(12 * PAGE_SIZE, 0);
        vpgd = (pgd_t) phys_to_virt(pgd);

        /* clear the page directory */
        for (i = 0; i < 1024; i++)
          {
            vpgd[i] = 0 | 2;
          };

        paddr = 0;
        j = PAGE_OFFSET;
        j >>= 22;
        ::cout << "[ PageFactory ] : j = 0x" << (int) j << "\n";

        vpmd = vpgd + PAGE_SIZE / sizeof(pgd_t); // first 'pmd', just after 'pgd'

        /* next map first 11MB */
        for (k = 0; k < 11; k++)
          {
            for (i = 0; i < 1024; i++)
              {
                vpmd[i] = paddr | 3;
                paddr += 0x1000;
              };
            vpgd[j++] = (u32) virt_to_phys(vpmd) | (u32) 3;
            vpmd += PAGE_SIZE / sizeof(pgd_t); // next 'pmd'
          }

        ::cout << "[ PageFactory ] : INIT(), new PGD: 0x" << (int) vpgd
            << " => 0x" << (int) pgd << "\n";

        set_cr3((u32) pgd);
        // we are not sure that paging is on
        set_cr0(get_cr0() | CR0_PG);

        ::cout << "[ PageFactory ] : init(), new PGD: 0x" << (int) vpgd
            << " => 0x" << (int) get_cr3() << "\n";
        ::cout << kernel::Console::DEC;

      }

      int
      PageFactory::reserve(void *start, size_t size)
      {
        struct p_block *blk, *tmp;
        char *end;
        OUTDEBUG("[ PageFactory ]");
        if (0 == size)
          {
            //		printk("%s:%d: ERROR! Invalid size: %d\n", __FILE__, __LINE__, size);
            return -EINVAL;
          }
        start = phys_to_virt(start);
        end = (char *) PAGE_ALIGN((char *) start + size);
        start = (void *) PAGE_TRUNC(start);
        size = (size_t)((end - (char*) (start)));
        blk = page_head.next;
        for (;;)
          {
            //		printk(">>>>>>>>>>>\n");
            if (blk == &page_head)
              {
                //			printk("%s: >>> ERROR failed to reserve page\n", __FUNCTION__);
                return -ENOMEM;
              }
            if ((char *) blk <= (char *) start && end <= (char *) blk
                + blk->size)
              break;
            blk = blk->next;
          }
        if ((char*) (blk) == (char*) (start) && blk->size == size)
          {
            blk->prev->next = blk->next;
            blk->next->prev = blk->prev;
          }
        else
          {
            if ((char*) (blk) + blk->size != end)
              {
                tmp = (struct p_block*) (end);
                tmp->size = (size_t)(((char*) (blk) + blk->size - end));
                tmp->next = blk->next;
                tmp->prev = blk;
                blk->size -= tmp->size;
                blk->next->prev = tmp;
                blk->next = tmp;
              }
            if ((char*) (blk) == (char*) (start))
              {
                blk->prev->next = blk->next;
                blk->next->prev = blk->prev;
              }
            else
              blk->size = (size_t)(((char*) (start) - (char*) (blk)));

          }
        used_bytes += size;
        return 0;
      }

      int
      PageFactory::cut(void *start, size_t size)
      {
        struct p_block *blk, *tmp;
        char *end, *_end, _start;
        if (size <= 0)
          {
            ::cout << "[ PageFactory ] cut(): ERROR! Invalid size \n";
            return -EINVAL;
          }
        cout << "[ PageFactory ] cut(): CUTTING PAGES\n";
        start = phys_to_virt(start);
        end = (char *) PAGE_ALIGN((char *) start + size);
        start = (void *) PAGE_TRUNC(start);
        size = (size_t)((end - (char*) (start)));
        blk = page_head.next;
        for (;;)
          {
            if (blk == &page_head)
              {
                return -ENOMEM;
              }

            _end = (char *) ((char *) blk + blk->size);

            ::cout << "[ PageFactory ]\n";

            if ((char *) blk <= (char *) start && (char *) start < _end)
              {
                if (end < _end)
                  {
                    ::cout
                        << "[ PageFactory ] start -> {blk:_end}, {end < _end}\n";
                    /* split block */
                    tmp = (struct p_block *) end;
                    tmp->size = (size_t)(_end - end);
                    tmp->next = blk->next;
                    tmp->next->prev = tmp;
                    tmp->prev = blk;

                    blk->size = (char *) start - (char *) blk;
                    blk->next = tmp;
                  }
                else
                  {
                    ::cout << "[ page ] start -> {blk:_end}, {end < _end}\n";
                    blk->size = (char *) start - (char *) blk;
                    start = _end;
                  }
              }
            else if ((char *) blk < end && end <= _end)
              {
                tmp = (struct p_block *) end;
                tmp->size = (size_t)(_end - end);
                tmp->prev = blk->prev;
                tmp->next = blk->next;
                tmp->next->prev = tmp;
                tmp->prev->next = tmp;
              }
            else if ((char *) start < (char *) blk && _end <= end)
              {
                /* remove page block completely */
                blk->prev->next = blk->next;
                blk->next->prev = blk->prev;
              }

            blk = blk->next;
          }
        return 0;
      }

      int
      PageFactory::free(void *addr, size_t size)
      {
        struct p_block *blk, *prev;
        if (addr == 0)
          {
            cout << "[ PageFactory ] Invalid parameter addr\n";
          }
        if (size == 0)
          {
            cout << "Invalid parameter size\n";
          }
        size = (size_t) PAGE_ALIGN(size);
        blk = (struct p_block *) phys_to_virt(addr);
        for (prev = &page_head; prev->next < blk; prev = prev->next)
          {
            if (prev->next == &page_head)
              break;

          }
        blk->size = size;
        blk->prev = prev;
        blk->next = prev->next;
        prev->next->prev = blk;
        prev->next = blk;
        if (blk->next != &page_head && ((char*) (blk) + blk->size)
            == (char*) (blk->next))
          {
            blk->size += blk->next->size;
            blk->next = blk->next->next;
            blk->next->prev = blk;
          }
        if (blk->prev != &page_head && (char*) (blk->prev) + blk->prev->size
            == (char*) (blk))
          {
            blk->prev->size += blk->size;
            blk->prev->next = blk->next;
            blk->next->prev = blk->prev;
          }
        used_bytes -= size;
        return 0;
      }

      /**
       * TODO Do page fit allocator to hold all the memory available
       *
       */
      void *
      PageFactory::alloc(size_t size, int align_order)
      {
        struct p_block *blk, *tmp;
        size_t _new_siz;
        OUTDEBUG("[ PageFactory ]");
        cout << "[ PageFactory ] Allocating 0x" << (int) (size) << " bytes\n";
        if (size == 0)
          {
            ::cout << "[ PageFactory ] alloc() Invalid parameter size\n";
            return NULL;
          }

        size = (size_t) PAGE_ALIGN(size);
        blk = &page_head;
        do
          {
            blk = blk->next;
            if (blk == &page_head)
              {
                // FIXME !!
                //sched_resume();
                ::cout << "[ PageFactory ] alloc(): out of memory\n";
                return NULL; /* Not found. */
              }
            tmp = (p_block *) ((paddr_t) blk & ~(PAGE_SIZE << align_order));
            _new_siz = (int) tmp - (int) blk + size;
          }
        while (blk->size < _new_siz);
        if (_new_siz > size)
          {
            tmp->next = blk->next;
            tmp->next->prev = tmp;
            blk->next = tmp;
            tmp->prev = blk;
            tmp->size = blk->size - _new_siz;
            blk->size = _new_siz;
            blk = tmp;
          }
        if (blk->size == size)
          {
            blk->prev->next = blk->next;
            blk->next->prev = blk->prev;
          }
        else
          {
            tmp = (struct p_block*) (((char*) (blk) + size));
            tmp->size = blk->size - size;
            tmp->prev = blk->prev;
            tmp->next = blk->next;
            blk->prev->next = tmp;
            blk->next->prev = tmp;
          }
        used_bytes += size;
        return virt_to_phys(blk);
      }

      /**
       *
       */
      PageFactory &
      PageFactory::operator <<(kernel::cmd::CMD mode)
      {
        switch (mode)
          {
        case kernel::cmd::INIT:
          init();
          break;
        default:
          break;
          }
        return *this;
      }

    }
  }
}

